#include "stdio.h"

#include <iostream>
#include <fstream>

#include "..\..\cGraph.h"
#include "..\..\cPath.h" 

using namespace std;

bool loadGraph(char *filename, cGraph& graph )
{
	return graph.Load(filename);
}

void printGraph(cGraph *graph)
{
	NodeMap tMap = *(graph->GetNodeMap());
	NodeMap::iterator it;

	for(it = tMap.begin(); it!=tMap.end(); ++it)
		{
			printf("%s [%d,%d] - %d connection(s)\n",it->first.c_str(),it->second->x,it->second->y,it->second->neighbours->size());

			ConnectionList *neighbours = it->second->neighbours;
			ConnectionList::iterator cit;

			for(cit = neighbours->begin(); cit!=neighbours->end(); ++cit)
			{
				printf("\tto %s for a cost of %d\n",(*cit)->node->id.c_str(),(*cit)->cost);
			}
		}
}

bool readUserInput(char **from, char **to)
{
	return true;
}

void printPath(std::vector< void* > *path)
{
	for(int i = 0; i < path->size(); i++ )
	{
		if(i >0 )
			printf(" ->");

		printf(" %s", ((cNode*)path->at(i))->id.c_str());
	}
}

int main(int argc, char *argv[])
{
	cGraph walkGraph;

	printf("Welcome to A* Finder - Powered by MicroPather\n\n");

	if(argc != 6)
	{
		printf("ERROR - Usage : AStarPathfinde filename sx sy ex ey");
		return -1;
	}

	printf("Loading file %s\n",argv[1]);

	bool loaded = loadGraph(argv[1], walkGraph);

	if(!loaded)
		return -1;

	printf("Graph:\n");

	printGraph(&walkGraph);

	char *from;
	char *to;

	readUserInput(&from, &to);

	std::vector< void* > path;
	float totalCost;

	int start_x;
	int start_y;
	int end_x;
	int end_y;

	sscanf(argv[2],"%d",&start_x);
	sscanf(argv[3],"%d",&start_y);
	sscanf(argv[4],"%d",&end_x);
	sscanf(argv[5],"%d",&end_y);

	printf("\n");
	printf("[%d,%d] -> [%d,%d]", start_x,start_y,end_x,end_y);
	printf("\n");

	//cNode *start = walkGraph.GetNodeMap()->find(string(argv[2]))->second; 
	//cNode *end = walkGraph.GetNodeMap()->find(string(argv[3]))->second;

	//int result = walkGraph.Find(start, end, &path, &totalCost);
	int result = walkGraph.Find(start_x, start_y, end_x, end_y, &path, &totalCost);

	if(result == MicroPather::SOLVED)
	{
		printf("Fount at cost %f\n\n",totalCost);
		printPath(&path);
	}
	else
	{
		printf("Not found!\n");
	}

	printf("\nCatmull-Rom interpolation steps (at %f), # nodes %d:\n",STEP_LENGTH,path.size());
	
	cPath walkingPath;

	int nPoints = walkingPath.Init(&path);

	if(nPoints < 2)
	{
		printf("at least 2 points are needed, cannot interpolate %d points!",nPoints);
		return -1;
	}

	int x;
	int y;
	int step = 0;
	int move;

	walkingPath.SetSpeed(10.0f);

	do
	{
		move=walkingPath.NextStep(&x, &y);

		printf("[%d] - [%d - %d]\n",step,x,y);

		if(move==ARRIVE)
		{
			walkingPath.Done();
		}
		else if(move==CONTINUE)
		{
			step++;
		}
	}
	while(!walkingPath.IsDone());

	return 0;
}
